// Aug. 6, 2011
// Author: Todd Miller
// Desc.:  Graphic display of Quadcopter, allows user to modify a percent pwm value for each motor.
//         The values are sent via the COM port selected at program start up.
//         See the Arduino program:  readSerial.pde for the setup to read the values that are sent.

import processing.serial.*;

// Serial Port:
Serial myPort;

boolean serialSel = true;
boolean comSel = false;
boolean baudSel = false;
int com = -1;
int baud = -1;
String opt;
String rate;

int scrWidth = 400;
int scrHeight = 500;
PImage logo;
PImage copter;
int padding;  // Declare variable to center logo across top of screen
color bgnd = 255;

//From: http://www.rapidtables.com/web/color/RGB_Color.htm
color fillColor1 = color(51,153,255);  // the blue color used
color fillColor2 = color(175,230,25);  // the green color used
color hlColor = color(255,255,100);    // soft yellow color
color fillText = 0;

int frontPercent = 0;
int backPercent = 0;
int leftPercent = 0;
int rightPercent = 0;

boolean frontSel = false;
boolean backSel = false;
boolean leftSel = false;
boolean rightSel = false;

color frontFill = 255;
color backFill = 255;
color leftFill = 255;
color rightFill = 255;

int i;




void setup()
{
  //currentcolor = fillColor1;
  size(scrWidth, scrHeight);
  logo = loadImage("usiieeelogo.jpg");
  copter = loadImage("ArduCopterQuad.jpg");
  println("logo width = " + logo.width + " logo height = " + logo.height);
  //noLoop();  
 
  // prompt user to select serial port, baud rate, parity and data bits:
  println(Serial.list());
  serialSel = true;
  
//  serialSetup();
} 

void draw()
{
  if(!serialSel)
  {
    background(bgnd);
    stroke(255);
    padding = (scrWidth - logo.width)/2;
    image(logo,padding,0); 
    image(copter,0,logo.height);
  
    // By Trial and Error
    // Front Motor
    drawFrontMotor();
  
    // Back Motor
    drawBackMotor();
  
    // Left Motor
    drawLeftMotor();
  
    // Right Motor
    drawRightMotor();
  }
  else
  {
    serialSetup();
  }

}

void serialSetup()
{
  background(bgnd);
  fill(fillText);
  text("Select COM Port: \n",10,20);
  for(i = 0; i < Serial.list().length; i++)
  {
    fill(fillText);
    text("[" + i + "] " + Serial.list()[i],10,20+(i+1)*15);
  }
  fill(fillText);
  text("\n\nSelect Baud Rate: \n[A] 115200\n[B] 38400\n[C] 9600\n",10,60);
 
  
    if(!(com == -1))
    {
      comSel = true;
    }
    if(!(baud == -1))
    {
      baudSel = true;
    }
  
  if(comSel && baudSel)
  {
    myPort = new Serial(this, Serial.list()[com], baud);
    serialSel = false;
  }
}


// CURRENTLY NOT COMPLETED
void reset()
{
  serialSel = true;
  comSel = false;
  baudSel = false;
  com = -1;
  baud = -1;
  for(i=frontPercent;i>0;i--)
  {
    frontPercent -= 5;
    delay(3000); 
  }
  backPercent = 0;
  leftPercent = 0;
  rightPercent = 0;
}

void drawFrontMotor()
{
  fill(frontFill);            // fill color to indicate motor selected
  ellipse(199,187,80,80);     // motor selected indicator
  fill(fillColor2);           // image graphics need to be redrawn over the indicator graphics
  ellipse(199,187,25,25);     // small inner circle on the motor
  fill(fillColor2);            
  rect(195,200,7,35);          // redraw inner portion of motor arm
  
  fill(fillText);
  text(frontPercent + "%",189,173);
}

void drawBackMotor()
{
   fill(backFill);              // fill to indicate motor selected
  ellipse(199,height-56,80,80); // motor selected indicator
  fill(fillColor2);            // fill to match image graphics covered by indicator graphics
  ellipse(199,height-56,25,25);  // small inner circle on motor
  fill(fillColor2);            
  rect(195,height-103,7,35);      // redraw inner portion of motor arm
  
  fill(fillText);
  text(backPercent + "%",189,height-30);
}

void drawLeftMotor()
{
  fill(leftFill);          // fill color to indicate motor selected
  ellipse(55,314,80,80);    // motor selected indicator
  fill(fillColor1);        // image graphics need to be redrawn over the indicator graphics
  ellipse(55,314,25,25);    // small inner circle on the motor
  fill(fillColor2);
  rect(68,311,35,6);        // redraw inner portion of motor arm
  
  fill(fillText);
  text(leftPercent + "%",15,317);
}

void drawRightMotor()
{
  fill(rightFill);                  // fill color to indicate motor selected
  ellipse(width-59,314,80,80);      // motor selected indicator
  fill(fillColor1);                 // image color (blue)
  ellipse(width-59,314,25,25);      // redraw inner circle on top of indicator
  fill(fillColor2);                 // image color (green)
  rect(width-105,311,33,6);          // redraw motor arm on top of inidcator
  
  fill(fillText);
  text(rightPercent + "%",width-45,317);
}

void keyPressed() 
{
  if(!serialSel)
  {
    if(key == 49) 
    {
      // 1 was pressed so toggle control to the front motor 
      if(frontSel == true)
      {
         frontSel = false; 
         frontFill = bgnd;
      }
      else if(frontSel == false)
      {
        frontSel = true;
        frontFill = hlColor;       
      }
    }
    if(key == 50)
    {
     if(backSel == true)
     {
       backSel = false;
       backFill = bgnd; 
     }
     else if(backSel == false)
     {
       backSel = true;
       backFill = hlColor;
     }
    }
    if(key == 51)
    {
      if(leftSel == true)
      {
        leftSel = false;
        leftFill = bgnd;
      }
      else if(leftSel == false)
      {
        leftSel = true;
        leftFill = hlColor;
      }
    }
    if(key == 52)
    {
      if(rightSel == true)
      {
        rightSel = false;
        rightFill = bgnd;
      }
      else if(rightSel == false)
      {
        rightSel = true;
        rightFill = hlColor;
      }
    }
  
    if(key == 83 || key == 115)    //'s' or 'S'
    {
      // go back into serial setup
      serialSel = true;
      comSel = false;
      baudSel = false;
      com = -1;
      baud = -1;
    }
    
    if (key == CODED) 
    {
      if (keyCode == UP) 
      {
        if(frontSel)
        {
          if(frontPercent < 100)
          {
            frontPercent += 1;
          
          }
        }
      
        if(backSel)
        {
          if(backPercent < 100)
          {
            backPercent += 1;
          }
        }
      
        if(leftSel)
        {
          if(leftPercent < 100)
          {
            leftPercent += 1;
          }
        }
        if(rightSel)
        {
          if(rightPercent < 100)
          {
            rightPercent += 1;
          }
        }      
      } 
      else if (keyCode == DOWN) 
      {
        if(frontSel)
        {
          if(frontPercent > 0)
          {
            frontPercent -= 1;
          }
        }
      
        if(backSel)
        {
          if(backPercent > 0)
          {
            backPercent -= 1;
          }
        }
      
        if(leftSel)
        {
          if(leftPercent > 0)
          {
            leftPercent -= 1;
          }
        }
        if(rightSel)
        {
          if(rightPercent > 0)
          {
            rightPercent -= 1;
          }
        }   
      }
    } 
    else 
    {
      //fillVal = 126;
    }
    //write out the motor values after a keyUp or keyDown press (SHOULD BE ABLE TO READ IN 4 BYTES AT A TIME IN THE REVERSE ORDER SHOWN BELOW)
    //
    myPort.write(rightPercent);
    myPort.write(leftPercent);
    myPort.write(backPercent);
    myPort.write(frontPercent);
  }
  else
  {
   // do some stuff for serial port setup 
   switch(key)
   {
     case 48:      //'0'
       com = 0;
       break;
     case 49:      //'1'
       com = 1;
       break;
     case 50:      //'2'
       com = 2;
       break;
     case 51:      //'3'
       com = 3;
       break;
     case 52:      //'4'
       com = 4;
       break;
     case 65:      //'A'
       baud = 115200;
       break;
     case 66:      //'B'
       baud = 38400;
       break;
     case 67:      //'C'
       baud = 9600;
       break;
   }
  }
}

